%找到最近的绿点
%输入：车坐标(1*2),空闲绿点坐标
%输出：到最近的绿点[步数，路径(n*2)]

function [STEP,PATH]=FindNearestGreen(inputCar,inputGreen)

[m,n]=size(inputGreen);
STEP=10000;
FLAG=1;
for i=1:1:m
    MobileOrder=GetTrajectory([inputCar(1,:);inputGreen(i,:)]);
    [m1,n1]=size(MobileOrder);
    step=m1-1;
    if step<STEP
        STEP=step;
        FLAG=i;
    end
end

PATH=GetTrajectory([inputCar(1,:);inputGreen(FLAG,:)]);



end